#pragma once
#include "ceres/autodiff_cost_function.h"
struct ErrorRelationType{
    Eigen::Vector3d origin_point;
    Eigen::Vector3d align_to_front_point;
    Eigen::Vector3d align_to_back_point;
};
class AlignErrorTerm
{
private:
    ErrorRelationType relation;
public:
    AlignErrorTerm(const ErrorRelationType & relation_):relation(relation_){}
    ~AlignErrorTerm(){};
    template<typename T>
    bool operator()(const T* const pos_ptr,
                const T* const rot_ptr,
                T* residuals_ptr)const{
        Eigen::Map<const Eigen::Matrix<T,3,1>>pos(pos_ptr);
        Eigen::Map<const Eigen::Quaternion<T>>rot(rot_ptr);
        Eigen::Matrix<T,3,1> front_point = rot*relation.align_to_front_point.template cast<T>() +pos;
        // front_point(2,0)=T(0);

        Eigen::Matrix<T,3,1> back_point = rot*relation.align_to_back_point.template cast<T>()+pos;
        // back_point(2,0)=T(0);
        T dis = (front_point-back_point).norm();
        if (dis<0.2)
        {
           Eigen::Matrix<T,3,1> avg_pos = (front_point+back_point)*0.5;
            *residuals_ptr =  (relation.origin_point.template cast<T>()-avg_pos).norm();
        }else{
            Eigen::Matrix<T,3,1> m = relation.origin_point.template cast<T>()- back_point;
            Eigen::Matrix<T,3,1>  n = back_point-front_point;

            *residuals_ptr = ( m.cross(n)).norm()/n.norm();
        }
        
        return true;
    }
    static ceres::CostFunction* Create(const ErrorRelationType & relation_) {
        return new ceres::AutoDiffCostFunction<AlignErrorTerm, 1, 3, 4>(new AlignErrorTerm(relation_));
    }
};
